#include "xfcanetdriver/canetSend.h"

XfCanetSend::XfCanetSend(ros::NodeHandle &m_handle)
{ // 构造函数初始化UDP端口

  ros::NodeHandle private_nh("~");

  private_nh.param<std::string>("m_pcip", m_pcip, "192.168.1.4");
  private_nh.param<int>("m_pcport", m_pcport, 4001);

  private_nh.param<std::string>("m_canetip", m_canetip, "192.168.1.178");
  private_nh.param<int>("m_canetport", m_canetport, 4001);

  private_nh.param<int>("max_speed_PWM", max_speed_PWM, 2000);
  private_nh.param<float>("diff_wheelSpacing", Diff_wheelSpacing, 0.6); // 主动轮轮距，单位：m

  private_nh.param<int>("bdebug", bdebug, 0);

  m_speed_r = 0;
  m_speed_l = 0;
  m_bl = 0;

  if (bdebug > 0)
  {
    // paramters
    std::cout << FYEL("*****xfcanet-2e-u:parameters*******************")
              << std::endl;
    std::cout << FGRN("canet_ip: ") << m_canetip << std::endl;
    std::cout << FGRN("canet_port: ") << m_canetport << std::endl;
    std::cout << FGRN("pc_ip: ") << m_pcip << std::endl;
    std::cout << FGRN("pc_port: ") << m_pcport << std::endl;
    std::cout << FYEL("*****xfcanet:parameters end***************") << std::endl;
  }

  // sub_control = m_handle.subscribe("/xfcontrol", 1000, &XfCanetSend::callback_control, this); // 订阅节点
  sub_cmd_vel = m_handle.subscribe("/cmd_vel", 1, &XfCanetSend::callback_cmd_vel, this); // 订阅节点
  // pub_candriver = m_handle.advertise<xfcanetdriver::canInfo>("/xfcandriver", 1000);           // 发布节点
}

XfCanetSend::~XfCanetSend() // 析构函数，在停止对象调用时调用
{
  for (int i = 0; i < 100; i++)
  {
    SendCarControlInfoKernel(0, 0, 0, 0);
    usleep(10000); //10ms
  }
  printf("STOPPING CAR!\n");
  delete boost_udp; // 释放UDP调用内存
}

void XfCanetSend::run()
{

  // io
  boost::asio::io_service io_service;
  boost_udp = new Boost_UDP_Send(io_service, m_pcip, 0, m_canetip, m_canetport); // m_pcport=0:发送使用随机UDP端口

  ROS_INFO("XfCanetSend running! ");

  ros::Rate rate(1000);
  while (ros::ok())
  {
    ros::spinOnce();
    //  f =  ros::Time().now().toSec();

    SendCarControlInfoKernel(m_speed_l, m_speed_r, m_gear, m_bl);
    // SendCarControlInfoKernel(500, 200, 1, 0);

    //  s = ros::Time().now().toSec();
    rate.sleep();
  }
}
// 发送指令
void XfCanetSend::SendCarControlInfoKernel(short speed_l, short speed_r, short gear, short bl)
{

  unsigned char send_buf[13] = {0};

  // 发送
  // memset(&send_buf, 0, sizeof(send_buf));
  send_buf[0] = 0x08; // 标准帧
  send_buf[1] = 0x00;
  send_buf[2] = 0x00;
  send_buf[3] = 0x05; // id:0x501
  send_buf[4] = 0x01;

  send_buf[5] = 0xFF; // 全部使能
  send_buf[6] = gear;
  send_buf[7] = speed_l >> 8; // 高八位
  send_buf[8] = speed_l & 0xFF;
  send_buf[9] = speed_r >> 8;
  send_buf[10] = speed_r & 0xFF;
  send_buf[11] = bl;
  send_buf[12] = 0x00;
  int ret = boost_udp->send_data(send_buf, 13); // 通过UDP发送数据
}

void XfCanetSend::callback_control(const xfcanetdriver::controlresult::ConstPtr &msg) // 订阅control
{
  float anglecha = 0; // 转30度，发--90~90,左正右负
  float gasstep = 0;  // 中心速度，发-100～100
  aimaction = *msg;
  // 上层来的转角和油门
  anglecha = aimaction.anglecha;
  if (aimaction.gear == 1)
  {
    m_gear = 1;
    gasstep = aimaction.cc_speed;
  }
  else if (aimaction.gear == 2)
  {
    m_gear = 2;
    gasstep = -aimaction.cc_speed;
  }
  else
  {
    m_gear = 0;
    gasstep = 0;
  }
  if (gasstep > 100)
    gasstep = 100;

  // bl = aimaction.breakstep; //接收制动

  if (aimaction.cc_speed != 0 && aimaction.gear != 0)
  {
    if (aimaction.anglecha > 40)
    {
      m_speed_r = gasstep * 3 + aimaction.anglecha * 9;
      m_speed_l = gasstep * 3 - aimaction.anglecha * 9;
    }
    else
    {
      m_speed_r = gasstep * 5 + aimaction.anglecha * 3;
      m_speed_l = gasstep * 5 - aimaction.anglecha * 3;
    }
  }
  else if (aimaction.cc_speed == 0 || aimaction.gear == 0)
  {
    m_speed_l = 0;
    m_speed_r = 0;
  }

  // 限制速度范围
  if (m_speed_r > 2000)
    m_speed_r = 2000;
  if (m_speed_r < 0)
    m_speed_r = 0;
  if (m_speed_l > 2000)
    m_speed_l = 2000;
  if (m_speed_l < 0)
    m_speed_l = 0;

  if (bdebug > 0)
    ROS_INFO("speed_l:%d speed_r:%d gear:%d", (int)m_speed_l, (int)m_speed_r, m_gear);
}

float XfCanetSend::target_limit_float(float insert, float low, float high)
{
  if (insert < low)
    return low;
  else if (insert > high)
    return high;
  else
    return insert;
}
int XfCanetSend::target_limit_int(int insert, int low, int high)
{
  if (insert < low)
    return low;
  else if (insert > high)
    return high;
  else
    return insert;
}

void XfCanetSend::callback_cmd_vel(const geometry_msgs::Twist &twist_aux)
{
  float amplitude = 2.0; // 3.5; // Wheel target speed limit //车轮目标速度限幅

  float Vx = twist_aux.linear.x;
  float Vz = twist_aux.angular.z;

  // Inverse kinematics //运动学逆解
  float speed_l = Vx - Vz * Diff_wheelSpacing / 2.0f; // 计算出左轮的目标速度
  float speed_r = Vx + Vz * Diff_wheelSpacing / 2.0f; // 计算出右轮的目标速度

  // Wheel (motor) target speed limit //车轮(电机)目标速度限幅
  speed_l = target_limit_float(speed_l, -amplitude, amplitude);
  speed_r = target_limit_float(speed_r, -amplitude, amplitude);

  speed_l = speed_l * max_speed_PWM / amplitude; // m/s to PWM, limit to amplitude m/s
  speed_r = speed_r * max_speed_PWM / amplitude;

  if (Vx > 0)
  {
    m_gear = 1;
    if (speed_l < 0)
      speed_l = 0;
    if (speed_r < 0)
      speed_r = 0;
  }
  else if (Vx < 0)
  {
    m_gear = 2;
    if (speed_l > 0)
      speed_l = 0;
    else
      speed_l = -speed_l;

    if (speed_r > 0)
      speed_r = 0;
    else
      speed_r = -speed_r;
  }
  else
  {
    m_gear = 0;
    speed_l = 0;
    speed_r = 0;
  }

  m_speed_l = target_limit_float(speed_l, 0, max_speed_PWM);
  m_speed_r = target_limit_float(speed_r, 0, max_speed_PWM);

  if (bdebug > 0)
    ROS_INFO("speed_l:%d speed_r:%d gear:%d", (int)m_speed_l, (int)m_speed_r, m_gear);
}